#!/usr/bin/env python3
"""
@Author: xueyuankui
@Date:   2021-07-15
@email: xueyuankui.good@163.com
@Function: The API used for mavlink communication with PX4 Fcu through UDP or Serial
"""
import time
import serial
import socket
import threading
import copy
import math

from .mavlink.mavlink_common import *
from .utils import rotation_transform as rf
from .px4_common import *
from .fcu_interface import FcuInterface

##############################################################################
# Device class
##############################################################################


class Device(object):
    def __init__(self):
        self.dev = None

    def write(self, buf):
        pass

    def read(self, size):
        pass

    def close(self):
        self.dev.close()


class DeviceSerial(Device):
    """
    Driver for link device by serial
    """

    def __init__(self, dev_n, baud):
        super().__init__()
        self.dev = serial.Serial(dev_n, baud, timeout=None)

    def write(self, buf):
        send_callback = self.dev.write(buf)

    def read(self, size):
        buf = self.dev.read(size)
        return buf


class DeviceUdp(Device):
    """
    Driver for link device by UDP
    """

    def __init__(self, local_addr, target_addr=None):
        """
        local_port: local port to receive data
        For example, target_addr = ('192.168.4.1', 18750)
        """
        super().__init__()
        self.local_addr = local_addr
        self.target_addr = target_addr
        self.source_addr = None

        self.dev = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.dev.bind(self.local_addr)

    def write(self, buf):
        if self.target_addr:
            send_callback = self.dev.sendto(buf, self.target_addr)
        elif self.source_addr:
            send_callback = self.dev.sendto(buf, self.source_addr)
        else:
            print("ERROR: unknown address to send")

    def read(self, size):
        # print(f"source_addr={self.source_addr}")
        buf, self.source_addr = self.dev.recvfrom(size)
        return buf

##############################################################################
# PX4 MAVLink
##############################################################################


class FcuPx4(FcuInterface):
    """
    MAVLink API Class.
    """

    def __init__(self, fcu_url, params_dict={}):
        """
        - Serial: serial:///path_to_serial_dev[:baudrate], for example serial:///dev/ttyACM0:115200
        - UDP: udp://:localhost[@bind_host][:bind_port], for example udp://:14550, udp://:14550@192.168.1.12:18570
        - TCP: tcp://[server_host][:server_port]
        """
        ###
        super(FcuInterface, self).__init__()
        ###
        # system id of onboard computer
        self.system_id = params_dict['sys_id']
        # component id of onboard computer
        self.component_id = params_dict['cmp_id']

        # the target device sys id you want to control and send COMMAND_LONG message to
        self.tgt_system_id = params_dict['tgt_sys_id']
        # the target device cmp id you want to control and send COMMAND_LONG message to
        self.tgt_component_id = params_dict['tgt_cmp_id']

        self.src_system_id = 0         # fcu system id, received in heartbeat message
        self.src_component_id = 0         # fcu component id, received in heartbeat message

        ###
        self.fcu_dev = None
        if 'serial' in fcu_url:
            str_parts = fcu_url.split(":")
            dev_n = str_parts[1][2:]
            baud = int(str_parts[2])
            # print("INFO, {}, {}".format(dev_n, baud))
            self.fcu_dev = DeviceSerial(dev_n, baud)

        elif 'udp' in fcu_url:
            if '@' in fcu_url:
                str_parts = fcu_url.split("@")
                # print(str_parts[0], type(str_parts[0]))
                # print(str_parts[0].split(':'))
                local_port = int(str_parts[0].split(':')[2])
                tgt_ip = str_parts[1].split(':')[0]
                tgt_port = int(str_parts[1].split(':')[1])

                local_addr = ('', local_port)
                target_addr = (tgt_ip, tgt_port)
                # print(local_addr, target_addr)
                self.fcu_dev = DeviceUdp(local_addr, target_addr)
            else:
                str_parts = fcu_url.split(":")
                ip = str_parts[1][2:]
                port = int(str_parts[2])
                # print("INFO, {}, {}".format(ip, port))

                local_addr = ('', port)
                self.fcu_dev = DeviceUdp(local_addr)
        # fcu mavlink
        self.fcu_mavlink = None
        if self.fcu_dev:
            self.fcu_mavlink = MAVLink(
                self.fcu_dev, self.system_id, self.component_id)

        # gcs mavlink
        self.gcs_dev = None
        self.gcs_mavlink = None
        self.flag_enable_gcs_link = False
        if 'gcs_url' in params_dict:
            self.flag_enable_gcs_link = True
            self.__setup_gcs_link(params_dict['gcs_url'])

        # Common messages
        self.base_mode = 0
        self.custom_mode = 0
        self.autopilot = 0
        #
        self.battery_remain = 0.0
        self.battery_voltage = 0.0

        # Attitude
        # NED
        self.attitude_NED = (0.0, 0.0, 0.0)
        self.roll_NED = 0.0
        self.pitch_NED = 0.0
        self.yaw_NED = 0.0
        self.quaternion_NED = (1.0, 0.0, 0.0, 0.0)
        # ENU
        self.attitude_ENU = (0.0, 0.0, 0.0)
        self.roll_ENU = 0.0
        self.pitch_ENU = 0.0
        self.yaw_ENU = 0.0
        self.quaternion_ENU = (1.0, 0.0, 0.0, 0.0)

        # Position
        # local position
        self.local_position = (0.0, 0.0, 0.0)
        self.local_pos_x = 0.0
        self.local_pos_y = 0.0
        self.local_pos_z = 0.0
        self.local_vel_x = 0.0
        self.local_vel_y = 0.0
        self.local_vel_z = 0.0
        # global position
        self.global_position = (0.0, 0.0, 0.0)
        self.latitude = 0.0
        self.longitude = 0.0
        self.altitude = 0.0
        self.global_position_alt_rel = (0.0, 0.0, 0.0)
        self.latitude_raw = 0.0
        self.longitude_raw = 0.0
        self.altitude_raw = 0.0
        self.alt_rel = 0.0
        self.alt_amsl = 0.0
        self.alt_local = 0.0
        self.global_velocity = (0.0, 0.0, 0.0)
        self.global_vel_x = 0.0
        self.global_vel_y = 0.0
        self.global_vel_z = 0.0

        # home position
        self.flag_get_home = False
        self.home_global_position = (0.0, 0.0, 0.0)
        self.home_altitude = 0.0
        self.home_local_position = (0.0, 0.0, 0.0)

        ###
        self.cur_mav_msg = None
        # message updating system time
        self.msg_time_boot_ms = {'altitude': 0,
                                 'attitude': 0,
                                 'attitude_quaternion': 0,
                                 'attitude_target': 0,
                                 'battery_status': 0,
                                 'gps_raw_int': 0,
                                 'heartbeat': 0,
                                 'local_position_ned': 0,
                                 'system_time': 0,
                                 'global_position_int': 0
                                 }
        self.msg_time_sys = {'altitude': 0,
                             'attitude': 0,
                             'attitude_quaternion': 0,
                             'attitude_target': 0,
                             'battery_status': 0,
                             'gps_raw_int': 0,
                             'heartbeat': 0,
                             'local_position_ned': 0,
                             'system_time': 0,
                             'global_position_int': 0,
                             }
        self.msg_hz = {'altitude': 0.0,
                       'attitude': 0.0,
                       'attitude_quaternion': 0.0,
                       'attitude_target': 0.0,
                       'battery_status': 0.0,
                       'gps_raw_int': 0.0,
                       'heartbeat': 0.0,
                       'local_position_ned': 0.0,
                       'system_time': 0.0,
                       'global_position_int': 0.0,
                       }
        ###
        self.msg_type_waiting_for = None
        self.msg_waited_success = None
        self.flag_waited_success = False

        # threads
        # thread: receive from fcu
        self.flag_fcu_receive = True
        self.th_fcu_receive = threading.Thread(
            daemon=True, target=self.th_fcu_receive_fun)
        self.th_fcu_receive.start()
        # thread: transfer data to gcs,  fcu <--udp/serial--> onboard <---udp---> gcs
        self.flag_gcs_receive = False
        if self.flag_enable_gcs_link:
            self.th_gcs_receive = threading.Thread(
                daemon=True, target=self.th_gcs_receive_fun)
            self.flag_gcs_receive = True
            self.th_gcs_receive.start()

        # thread: heartbeat message send to fcu
        self.flag_send_heartbeat_msg = True
        self.th_send_heartbeat_msg = threading.Thread(
            daemon=True, target=self.th_send_heartbeat_msg_fun)
        self.th_send_heartbeat_msg.start()

        ###
        self.system_start_time = time.time()
        self.time_boot_ms = 0

    def __str__(self):
        info = ">>>>>>>>>>>>>>>>>FCU common message:<<<<<<<<<<<<<<<<<<\n"
        info += "FCU: sysid={}\n".format(self.tgt_system_id)
        info += "FCU: mode={}\n".format(self.custom_mode)
        info += "Battery: voltage={:.2f}, cap={:.2f}\n".format(
            self.battery_voltage, self.battery_remain)
        info += "GPS    : lat={:.6f}, lon={:.6f}, alt={:.3f}\n".format(
            self.latitude, self.longitude, self.altitude)
        info += "Local Pos: x={:.3f}, y={:.3f}, z={:.3f}\n".format(
            self.local_pos_x, self.local_pos_y, self.local_pos_z)
        return info

    def __setup_gcs_link(self, gcs_url):
        self.gcs_dev = None
        if 'udp' in gcs_url:
            if '@' in gcs_url:
                str_parts = gcs_url.split("@")
                # print(str_parts[0], type(str_parts[0]))
                # print(str_parts[0].split(':'))
                local_port = int(str_parts[0].split(':')[2])
                tgt_ip = str_parts[1].split(':')[0]
                tgt_port = int(str_parts[1].split(':')[1])

                local_addr = ('', local_port)
                target_addr = (tgt_ip, tgt_port)
                # print(local_addr, target_addr)
                self.gcs_dev = DeviceUdp(local_addr, target_addr)
            else:
                str_parts = gcs_url.split(":")
                ip = str_parts[1][2:]
                port = int(str_parts[2])
                # print("INFO, {}, {}".format(ip, port))

                local_addr = ('', port)
                self.gcs_dev = DeviceUdp(local_addr)
        self.gcs_mavlink = MAVLink(
            self.gcs_dev, self.tgt_system_id, self.tgt_component_id)

    def th_gcs_receive_fun(self):
        while self.flag_gcs_receive:
            data = self.gcs_dev.read(256)
            if len(data) > 0:
                # transfer data to fcu
                self.fcu_dev.write(data)
            msgs = []
            if len(data) > 0:
                try:
                    # just return one message
                    # self.cur_mav_msg = self.fcu_mavlink.parse_char(data)
                    # return a message list
                    msgs = self.gcs_mavlink.parse_buffer(data)
                    if msgs[0] is None:
                        continue
                    # print(len(msgs))
                    # print(msgs)
                except Exception as e:
                    continue

    def th_fcu_receive_fun(self):
        while self.flag_fcu_receive:
            data = self.fcu_dev.read(256)
            ###
            if len(data) > 0:
                if self.flag_enable_gcs_link:
                    self.gcs_dev.write(data)
            ###
            # print(data)
            msgs = []
            if len(data) > 0:
                try:
                    # just return one message
                    # self.cur_mav_msg = self.fcu_mavlink.parse_char(data)
                    # return a message list
                    msgs = self.fcu_mavlink.parse_buffer(data)
                    if msgs[0] is None:
                        continue
                    # print(len(msgs))
                except Exception as e:
                    continue
            for msg in msgs:
                self.cur_mav_msg = msg
                if isinstance(self.cur_mav_msg, MAVLink_message):
                    pass
                    # print(self.cur_mav_msg)
                if isinstance(self.cur_mav_msg, MAVLink_heartbeat_message):
                    # print("INFO: sys={}, cmp={}".format(self.cur_mav_msg.get_srcSystem(), self.cur_mav_msg.get_srcComponent()))
                    self.src_system_id = self.cur_mav_msg.get_srcSystem()
                    self.src_component_id = self.cur_mav_msg.get_srcComponent()
                    self.base_mode = self.cur_mav_msg.base_mode
                    self.custom_mode = self.cur_mav_msg.custom_mode

                    if self.msg_time_sys['heartbeat'] != 0:
                        self.msg_hz['heartbeat'] = 1 / \
                            (time.time() - self.msg_time_sys['heartbeat'])
                    self.msg_time_sys['heartbeat'] = time.time()

                if isinstance(self.cur_mav_msg, MAVLink_battery_status_message):
                    # print("INFO: remain={}, voltages={}".format(self.cur_mav_msg.battery_remaining, self.cur_mav_msg.voltages))
                    self.battery_remain = self.cur_mav_msg.battery_remaining
                    vol_total = 0.0
                    for vol in self.cur_mav_msg.voltages:
                        if vol != 65535:
                            vol_total += vol / 1000
                    self.battery_voltage = vol_total
                    # print(self.battery_voltage)
                    self.__update_msg_hz('battery_status')

                if isinstance(self.cur_mav_msg, MAVLink_attitude_message):
                    # print("INFO: roll={:.3f}, pitch={:.3f}, yaw={:.3f}".format(self.cur_mav_msg.roll, self.cur_mav_msg.pitch, self.cur_mav_msg.yaw))
                    self.roll_NED = self.cur_mav_msg.roll
                    self.pitch_NED = self.cur_mav_msg.pitch
                    self.yaw_NED = self.cur_mav_msg.yaw
                    self.attitude_NED = (
                        self.roll_NED, self.pitch_NED, self.yaw_NED)
                    time_boot_ms = self.cur_mav_msg.time_boot_ms
                    time_boot_ms = time.time()
                    if self.msg_time_boot_ms['attitude'] != 0:
                        try:
                            self.msg_hz['attitude'] = 1000 / \
                                (time_boot_ms -
                                 self.msg_time_boot_ms['attitude'])
                        except Exception as e:
                            # print(time_boot_ms, self.msg_time_boot_ms['attitude'])
                            print("ERROR: ", e)
                        self.msg_time_boot_ms['attitude'] = time_boot_ms

                    self.__update_msg_hz('attitude')

                if isinstance(self.cur_mav_msg, MAVLink_attitude_quaternion_message):
                    # print("INFO: quaternion w={:.3f} x={:.3f} y={:.3f} z={:.3f}".format(self.cur_mav_msg.q1, self.cur_mav_msg.q2, self.cur_mav_msg.q3, self.cur_mav_msg.q4))
                    self.quaternion_NED = [
                        self.cur_mav_msg.q1, self.cur_mav_msg.q2, self.cur_mav_msg.q3, self.cur_mav_msg.q4]
                    self.attitude_NED = rf.qbe2euler(self.quaternion_NED)
                    # self.roll_NED, self.pitch_NED, self.yaw_NED = self.attitude_NED[0], self.attitude_NED[1], self.attitude_NED[2]

                    self.quaternion_ENU = rf.qbe_NED2ENU(self.quaternion_NED)
                    self.attitude_ENU = rf.qbe2euler(self.quaternion_ENU)
                    self.roll_ENU, self.pitch_ENU, self.yaw_ENU = self.attitude_ENU[
                        0], self.attitude_ENU[1], self.attitude_ENU[2]

                    # print("INFO: attitude NED ", [a*180/math.pi for a in self.attitude_NED])
                    # print("INFO: attitude ENU ", [a*180/math.pi for a in self.attitude_ENU])
                    self.__update_msg_hz('attitude_quaternion')

                if isinstance(self.cur_mav_msg, MAVLink_gps_raw_int_message):
                    # print("INFO: lat={:.3f}, lon={:.3f}, alt={:.3f}".format(self.cur_mav_msg.lat, self.cur_mav_msg.lon, self.cur_mav_msg.alt))
                    self.latitude_raw = self.cur_mav_msg.lat / 1e7
                    self.longitude_raw = self.cur_mav_msg.lon / 1e7
                    self.altitude_raw = self.cur_mav_msg.alt
                    self.__update_msg_hz('gps_raw_int')

                if isinstance(self.cur_mav_msg, MAVLink_global_position_int_message):
                    # print("INFO: lat={:.3f}, lon={:.3f}, alt={:.3f}, alt_rel={:.3f}, vx={:.2f}, vy={:.2f}, vz={:.2f}".format(self.latitude, self.longitude, self.altitude, self.alt_rel, \
                    #                        self.global_vel_x, self.global_vel_y, self.global_vel_z))
                    self.latitude = self.cur_mav_msg.lat / 1e7
                    self.longitude = self.cur_mav_msg.lon / 1e7
                    self.altitude = self.cur_mav_msg.alt / 1000  # mm to m
                    self.alt_rel = self.cur_mav_msg.relative_alt / 1000  # mm to m
                    self.global_vel_x = self.cur_mav_msg.vx / 100  # cm/s to m/s
                    self.global_vel_y = self.cur_mav_msg.vy / 100  # cm/s to m/s
                    self.global_vel_z = self.cur_mav_msg.vz / 100  # cm/s to m/s
                    self.global_position = (
                        self.latitude, self.longitude, self.altitude)
                    self.global_position_alt_rel = (
                        self.latitude, self.longitude, self.alt_rel)
                    self.__update_msg_hz('global_position_int')

                if isinstance(self.cur_mav_msg, MAVLink_local_position_ned_message):
                    self.local_pos_x = self.cur_mav_msg.x
                    self.local_pos_y = self.cur_mav_msg.y
                    self.local_pos_z = self.cur_mav_msg.z
                    self.local_vel_x = self.cur_mav_msg.vx
                    self.local_vel_y = self.cur_mav_msg.vy
                    self.local_vel_z = self.cur_mav_msg.vz
                    self.local_position = (
                        self.local_pos_x, self.local_pos_y, self.local_pos_z)
                    self.__update_msg_hz('local_position_ned')

                if isinstance(self.cur_mav_msg, MAVLink_altitude_message):
                    # print("INFO: alt_rel={:.3f}, alt_amsl={:.3f}, alt_local={:.3f}".format(self.cur_mav_msg.altitude_relative, self.cur_mav_msg.altitude_amsl, self.cur_mav_msg.altitude_local))
                    self.alt_rel = self.cur_mav_msg.altitude_relative
                    self.alt_amsl = self.cur_mav_msg.altitude_amsl
                    self.alt_local = self.cur_mav_msg.altitude_local
                    self.__update_msg_hz('altitude')

                if isinstance(self.cur_mav_msg, MAVLink_home_position_message):
                    altitude = self.cur_mav_msg.altitude / 1000
                    self.home_global_position = (
                        self.cur_mav_msg.latitude / 1e7, self.cur_mav_msg.longitude / 1e7, altitude)
                    self.home_altitude = altitude
                    self.home_local_position = (
                        self.cur_mav_msg.x, self.cur_mav_msg.y, self.cur_mav_msg.z)
                    self.flag_get_home = True
                    # print(self.home_global_position)

                if self.msg_type_waiting_for is not None:
                    if isinstance(self.cur_mav_msg, self.msg_type_waiting_for):
                        self.msg_waited_success = copy.deepcopy(
                            self.cur_mav_msg)
                        self.flag_waited_success = True

    def th_send_heartbeat_msg_fun(self):
        """
        Send heartbeat message to drone
        """
        while self.flag_send_heartbeat_msg:
            msg = MAVLink_heartbeat_message(type=MAV_TYPE_GCS, autopilot=MAV_AUTOPILOT_INVALID, base_mode=192, custom_mode=0,
                                            system_status=MAV_STATE_ACTIVE, mavlink_version=3)
            self.fcu_mavlink.send(msg)
            time.sleep(1)

    def __update_msg_hz(self, msg_name):
        if msg_name not in self.msg_time_sys:
            return
        if self.msg_time_sys[msg_name] != 0:
            self.msg_hz[msg_name] = 1 / \
                (time.time() - self.msg_time_sys[msg_name])
        self.msg_time_sys[msg_name] = time.time()
        # print(self.msg_hz)

    def __wait_for_message(self, timeout=3):
        """
        Not very stable
        """
        start_time = time.time()
        while time.time() - start_time < timeout:
            if self.flag_waited_success:
                self.flag_waited_success = False
                return self.msg_waited_success

        return None

    def arm_disarm(self, arm):
        """
        arm:
            - False: disarm
            - True: arm
        """
        arm = 1 if arm else 0
        msg_cmd = MAVLink_command_long_message(
            self.tgt_system_id, self.tgt_component_id, MAV_CMD_COMPONENT_ARM_DISARM, 0, arm, 0, 0, 0, 0, 0, 0)
        self.fcu_mavlink.send(msg_cmd)

    def set_mode(self, custom_mode, base_mode=129):
        """
        set mode by sending MAVLink SET_MODE(#11) message
        """
        # base_mode = 1
        # base_mode = 129
        base_mode = base_mode
        custom_mode = int(custom_mode)

        msg = MAVLink_set_mode_message(
            self.tgt_system_id, base_mode=base_mode, custom_mode=custom_mode)

        # msg_cmd = MAVLink_command_long_message(self.tgt_system_id, self.tgt_component_id, MAV_CMD_DO_SET_MODE, 0, mode, custom_mode, custom_submode, 0, 0, 0, 0)
        # self.fcu_mavlink.send(msg_cmd)
        # self.fcu_mavlink.command_long_send(self.tgt_system_id, self.tgt_component_id, MAV_CMD_DO_SET_MODE, 0, mode, custom_mode, custom_submode, 0, 0, 0, 0)

        self.fcu_mavlink.send(msg)

    def setpoint_body_ned(self, type_mask, pvax, pvay, pvaz, yaw_value):
        """
        Setpoint in body frame
        """
        coordinate_frame = MAV_FRAME_BODY_NED
        # print(type_mask)
        if type_mask == PX4_TYPE_MASK.MASK_NED_VEL_YAWRATE:
            vx, vy, vz = pvax, pvay, pvaz
            msg = MAVLink_set_position_target_local_ned_message(0, self.tgt_system_id, self.tgt_component_id,
                                                                coordinate_frame, type_mask, 0, 0, 0, vx, vy, vz, 0, 0, 0, 0, yaw_value)
        elif type_mask == PX4_TYPE_MASK.MASK_NED_VEL_YAW:
            vx, vy, vz = pvax, pvay, pvaz
            msg = MAVLink_set_position_target_local_ned_message(0, self.tgt_system_id, self.tgt_component_id,
                                                                coordinate_frame, type_mask, 0, 0, 0, vx, vy, vz, 0, 0, 0, yaw_value, 0)
        elif type_mask == PX4_TYPE_MASK.MASK_NED_ACC_YAWRATE:
            ax, ay, az = pvax, pvay, pvaz
            msg = MAVLink_set_position_target_local_ned_message(0, self.tgt_system_id, self.tgt_component_id,
                                                                coordinate_frame, type_mask, 0, 0, 0, 0, 0, 0, ax, ay, az, yaw_value, 0)
        elif type_mask == PX4_TYPE_MASK.MASK_NED_ACC_YAW:
            ax, ay, az = pvax, pvay, pvaz
            msg = MAVLink_set_position_target_local_ned_message(0, self.tgt_system_id, self.tgt_component_id,
                                                                coordinate_frame, type_mask, 0, 0, 0, 0, 0, 0, ax, ay, az, yaw_value, 0)
        self.fcu_mavlink.send(msg)

    def setpoint_local_ned(self, type_mask, pvax, pvay, pvaz, yaw_value):
        """
        Setpoint in local frame
        """
        coordinate_frame = MAV_FRAME_LOCAL_NED
        if type_mask == PX4_TYPE_MASK.MASK_NED_VEL_YAWRATE:
            vx, vy, vz = pvax, pvay, pvaz
            msg = MAVLink_set_position_target_local_ned_message(0, self.tgt_system_id, self.tgt_component_id,
                                                                coordinate_frame, type_mask, 0, 0, 0, vx, vy, vz, 0, 0, 0, 0, yaw_value)
        elif type_mask == PX4_TYPE_MASK.MASK_NED_VEL_YAW:
            vx, vy, vz = pvax, pvay, pvaz
            msg = MAVLink_set_position_target_local_ned_message(0, self.tgt_system_id, self.tgt_component_id,
                                                                coordinate_frame, type_mask, 0, 0, 0, vx, vy, vz, 0, 0, 0, yaw_value, 0)
        elif type_mask == PX4_TYPE_MASK.MASK_NED_POS_YAW:
            px, py, pz = pvax, pvay, pvaz
            msg = MAVLink_set_position_target_local_ned_message(0, self.tgt_system_id, self.tgt_component_id,
                                                                coordinate_frame, type_mask, px, py, pz, 0, 0, 0, 0, 0, 0, yaw_value, 0)
        elif type_mask == PX4_TYPE_MASK.MASK_NED_ACC_YAWRATE:
            ax, ay, az = pvax, pvay, pvaz
            msg = MAVLink_set_position_target_local_ned_message(0, self.tgt_system_id, self.tgt_component_id,
                                                                coordinate_frame, type_mask, 0, 0, 0, 0, 0, 0, ax, ay, az, yaw_value, 0)
        elif type_mask == PX4_TYPE_MASK.MASK_NED_ACC_YAW:
            ax, ay, az = pvax, pvay, pvaz
            msg = MAVLink_set_position_target_local_ned_message(0, self.tgt_system_id, self.tgt_component_id,
                                                                coordinate_frame, type_mask, 0, 0, 0, 0, 0, 0, ax, ay, az, yaw_value, 0)
        self.fcu_mavlink.send(msg)

    def setpoint_global_ned_alt(self, type_mask, pvax, pvay, pvaz, yaw_value):
        """
        Not work yet
        """
        coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
        if type_mask == PX4_TYPE_MASK.MASK_NED_POS_YAW:
            lat = int(pvax * 1e7)
            lon = int(pvay * 1e7)
            rel_alt = pvaz
            time_boot_ms = int((time.time() - self.system_start_time) * 1000)
            msg = MAVLink_set_position_target_global_int_message(time_boot_ms, self.tgt_system_id, self.tgt_component_id,
                                                                 coordinate_frame, type_mask, lat, lon, rel_alt, 0, 0, 0, 0, 0, 0, yaw_value, 0)
        elif type_mask == PX4_TYPE_MASK.MASK_NED_VEL_YAWRATE:
            vx = pvax
            vy = pvay
            vz = pvaz
            msg = MAVLink_set_position_target_global_int_message(0, self.tgt_system_id, self.tgt_component_id,
                                                                 coordinate_frame, type_mask, 0, 0, 0, vx, vy, vz, 0, 0, 0, 0, yaw_value)
        self.fcu_mavlink.send(msg)

    def cmd_goto_global_position(self, lat, lon, alt, yaw, speed):
        """
        alt:   m
        speed: m/s
        yaw:   deg
        MAV_CMD_DO_REPOSITION(192)
        """
        msg = MAVLink_command_long_message(self.tgt_system_id, self.tgt_component_id, MAV_CMD_DO_REPOSITION, 0,
                                           speed, MAV_DO_REPOSITION_FLAGS_CHANGE_MODE, 0, yaw, lat, lon, alt)
        self.fcu_mavlink.send(msg)

    def cmd_get_home_position(self):
        """
        Get home position
        MAV_CMD_GET_HOME_POSITION(410)
        """
        if self.flag_get_home:
            return self.home_global_position

        msg = MAVLink_command_long_message(self.tgt_system_id, self.tgt_component_id, MAV_CMD_DO_REPOSITION, 0,
                                           0, 0, 0, 0, 0, 0, 0)
        self.fcu_mavlink.send(msg)

        self.msg_type_waiting_for = MAVLink_home_position_message
        msg = self.__wait_for_message()
        # print(msg)
        if msg is not None:
            self.home_global_position = (
                msg.latitude, msg.longitude, msg.altitude)
            self.home_altitude = msg.altitude
            self.flag_get_home = True
            # print(self.home_global_position)
            # print(self.home_local_position)
        else:
            pass
            # print('message is None')
        return self.home_global_position

    def cmd_do_orbit(self, lat, lon, alt, radius=10.0, velocity=1.0):
        # msg = MAVLink_command_long_message(self.tgt_system_id, self.tgt_component_id, MAV_CMD_DO_ORBIT, 0,
        #                                     radius,
        #                                     velocity,
        #                                     ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER,
        #                                     0,
        #                                     lat,
        #                                     lon,
        #                                     alt)MAV_FRAME_GLOBAL_INT
        msg = MAVLink_command_int_message(self.tgt_system_id, self.tgt_component_id, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, MAV_CMD_DO_ORBIT, 0, 0,
                                          radius,
                                          velocity,
                                          ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER,
                                          0,
                                          int(lat*1e7),
                                          int(lon*1e7),
                                          alt)
        self.fcu_mavlink.send(msg)

    def exit(self):
        if self.fcu_dev is not None:
            self.fcu_dev.close()
        if self.gcs_dev is not None:
            self.gcs_dev.close()

    def __del__(self):
        self.exit()
